#pragma once

#include "cartographer/rcm/imu/imu_edge.h"

#define IMU_OPT_WIN_SIZE 10

namespace cartographer
{
    class ImuIntegrateOptimizer
    {
    public:
        using Ptr = std::shared_ptr<ImuIntegrateOptimizer>;

        ImuIntegrateOptimizer(size_t winSize);
        ~ImuIntegrateOptimizer();

        void Clear();

        int Optimize(ImuOptNode &resNode);

        void Push(ImuOptNode::Ptr node);

        void CheckForOptimize();

        ImuOptNode::Ptr GetOptNode(const int64 poseTime);

    private:
        void ClearCeres();

        inline void SetArray(const ImuOptNode::Ptr &node, int i)
        {
            ceresPoses_[i][0] = node->P(0);
            ceresPoses_[i][1] = node->P(1);
            ceresPoses_[i][2] = node->P(2);
            ceresPoses_[i][3] = node->Q.x();
            ceresPoses_[i][4] = node->Q.y();
            ceresPoses_[i][5] = node->Q.z();
            ceresPoses_[i][6] = node->Q.w();

            ceresVelocityBias_[i][0] = node->V(0);
            ceresVelocityBias_[i][1] = node->V(1);
            ceresVelocityBias_[i][2] = node->V(2);
            ceresVelocityBias_[i][3] = node->Ba(0);
            ceresVelocityBias_[i][4] = node->Ba(1);
            ceresVelocityBias_[i][5] = node->Ba(2);
            ceresVelocityBias_[i][6] = node->Bg(0);
            ceresVelocityBias_[i][7] = node->Bg(1);
            ceresVelocityBias_[i][8] = node->Bg(2);
        }

        double **ceresPoses_ = NULL;
        double **ceresVelocityBias_ = NULL;

        /*double ceresPoses_[IMU_OPT_WIN_SIZE + 1][SIZE_POSE];
        double ceresVelocityBias_[IMU_OPT_WIN_SIZE + 1][SIZE_SPEEDBIAS];*/

        size_t dataSize_ = 11;
        std::deque<ImuOptNode::Ptr> poses_;
    };
} // namespace cartographer